/*
 * TesterMain.cpp
 *
 *  Created on: Jan 7, 2011
 *      Author: dt
 *      mount -t nfs -o nolock 10.0.1.11:/export/nfs test
 */



/*
 * Jingle call example
 * Copyright 2004--2005, Google Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; withogoogle::protobuf::MessageLiteut even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Tempe Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <iomanip>
#include <time.h>
#include <pthread.h>
#include <stdio.h>

#include "terk/qemotortraj.h"

#ifndef WIN32
#include <sys/types.h>
#include <sys/stat.h>   //get motor reference
#include <unistd.h>
#include <iomanip>
#include <sys/types.h>
#include <sys/wait.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <getopt.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#include <linux/types.h>
#include <linux/videodev.h>



#else
#include <direct.h>
//typedef _getcwd getcwd;#include <google/protobuf/io/zero_copy_input_stream_impl_lite.h>
#include "talk/base/win32.h"
#endif

#include "talk/examples/monarch-common/XMPPCallback.h"
#include "talk/examples/monarch-common/monarchController.h"
#include "talk/base/flags.h"
#include <google/protobuf/io/zero_copy_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl_lite.h>

//#include "control.h"
//#include "v4l2uvc.h"
//#include "control.h"
//#include "v4l2uvc.h"
#include "utils.h"



#include "TesterMain.h"

#include "monarchmediaengine.h"


const int left_wheel = 0;
const int right_wheel = 3;
struct control_data cd, *cdata;

const char* MONARCH_VIDEO_DEVICE = "/dev/vide0";
const char* MONARCH_AUDIO_DEVICE = "/tmp/audio";

TesterMain::TesterMain(std::string& username, std::string& password,  std::string& xmppServerAddress, bool& debug , unsigned int& msgsize, unsigned int& msginterval)
{
  msgsize_ = msgsize;
  msginterval_ = msginterval;
  media_engine_ = CreateMonarchClientMediaEngine(NULL,MONARCH_AUDIO_DEVICE,MONARCH_VIDEO_DEVICE,NULL);
  monarch_controller_ = new MonarchController( this , username, password, xmppServerAddress, debug,media_engine_);
  controller_state_ = new monarch::ControllerState();
  monarch_controller_->start();
}


cricket::MediaEngine* TesterMain::CreateMonarchClientMediaEngine(const char* voice_in,
                                            const char* voice_out,
                                            const char* video_in,
                                            const char* video_out) {
  cricket::MonarchMediaEngine* monarch_media_engine = new cricket::MonarchMediaEngine;
  // Set the RTP dump file names.
  if (voice_in) {
    monarch_media_engine->set_voice_input_filename(voice_in);
  }
  if (voice_out) {
    monarch_media_engine->set_voice_output_filename(voice_out);
  }
  if (video_in) {
    monarch_media_engine->set_video_input_filename(video_in);
  }
  if (video_out) {
    monarch_media_engine->set_video_output_filename(video_out);
  }

  // Set voice and video codecs. TODO: The codecs actually depend on
  // the the input voice and video streams.
  std::vector<cricket::AudioCodec> voice_codecs;
  voice_codecs.push_back(
      cricket::AudioCodec(0, "PCMU", 8000, 0, 1, 0));
  monarch_media_engine->set_voice_codecs(voice_codecs);

  std::vector<cricket::VideoCodec> video_codecs;
  video_codecs.push_back(
      cricket::VideoCodec(26, "JPEG", 640, 480, 16, 0));
  monarch_media_engine->set_video_codecs(video_codecs);

  return monarch_media_engine;
}


void TesterMain::onRosterAdd(std::string& username)
{
  std::cout << "ROSTER UPDATED" <<  username << "\n" ;
  if(msgsize_){
    monarch_controller_->requestTunnelTo(username, this);
  }
}


void TesterMain:: onChatMessageReceived(std::string& from, std::string& msg)
{
  std::cout << from << " says: "<< msg << std::endl;
}

void TesterMain::onXmppConnected()
{
  std::cout << "XMPP CONNECTED\n";
}


ConnectionCallback* TesterMain:: onIncomingTunnelRequest()
{
  //always accept requests
  return this;
}



//start sending data if we are are the src
void TesterMain:: onTunnelConnect(PipeEp* p)
{
  std::cout << "TUNNEL CONNECTED\n";
  pipe_ = p;

}

void TesterMain:: onTunnelDisconnect(PipeEp* p, bool withError)
{
  stop = true;
}


void TesterMain::onDataRecive(PipeEp* p, const char* buf, size_t size)
{

  bool header_good = (buf[0]=='>')&&(buf[1]=='>')&&(buf[2]=='>');

  int msg_length = 0;
  if(header_good)
  {
	  msg_length = buf[3] *256;
	  msg_length += buf[4];
  }
  else
	  return;
  int i = 0;
  std::cout << "GOT DATA of Length " << msg_length << " ";
  while(i < msg_length+3)
  {
  	  printf("%x ",buf[i]);
  	  i++;
  }
  std::cout << "\n";

  google::protobuf::io::ZeroCopyInputStream* zcis = new google::protobuf::io::ArrayInputStream(buf,size);
  zcis->Skip(5);

  if(!controller_state_->ParsePartialFromBoundedZeroCopyStream(zcis,msg_length))
  {
	  std::cout << "ERROR"<< buf << "\n";
  }else
  {
	  std::cout << "MSG DECODED"<< "\n";
	  //get motor reference
	  if(controller_state_->keyevent_size()>0)
	  {
	     key_event_ = &controller_state_->keyevent(0);
	     if(key_event_->keycode().find("UP",0) != std::string::npos)
	     {
	   	   CQEMotorTraj &motor = CQEMotorTraj::GetRef();
	   	   motor.Move(left_wheel, 1000, 40000, 8000);
	   	   motor.Move(right_wheel, -1000, 40000, 8000);
	     }
	     if(key_event_->keycode().find("DOWN",0) != std::string::npos)
		 {
		   CQEMotorTraj &motor = CQEMotorTraj::GetRef();
		   motor.Move(left_wheel, -1000, 40000, 8000);
		   motor.Move(right_wheel, 1000, 40000, 8000);
		 }
	  }
  }
}


int main(int argc, char **argv) {

   // Verify that the version of the library that we linked against is
   // compatible with the version of the headers we compiled against.
 //  GOOGLE_PROTOBUF_VERIFY_VERSION;

  // define options
  DEFINE_bool(a, false, "Turn on auto accept.");
  DEFINE_bool(d, false, "Turn on debugging.");
  DEFINE_int(msgsize, 0, "Automatically start a test connection and send packets of this size");
  DEFINE_int(msginterval, 100, "Interval Between Sent Packets.");
  DEFINE_bool(testserver, false, "Use test server");
  DEFINE_int(portallocator, 0, "Filter out unwanted connection types.");
  DEFINE_string(filterhost, NULL, "Filter out the host from all candidates.");
  DEFINE_string(s, "talk.google.com", "The connection server to use.");
  DEFINE_bool(help, false, "Prints this message");

  // parse options
  FlagList::SetFlagsFromCommandLine(&argc, argv, true);
  if (FLAG_help)
    {
      FlagList::Print(NULL, false);
      return 0;
    }

  //bool auto_accept = FLAG_a;
  bool debug = FLAG_d;
  unsigned int msgsize = (unsigned int) FLAG_msgsize;
  unsigned int msginterval = (unsigned int) FLAG_msginterval;
  bool test_server = FLAG_testserver;
  //int32 portallocator_flags = FLAG_portallocator;
  std::string server = FLAG_s;

  // parse username and password, if present
  std::string username;
  std::string password = "";

  std::string xmppServerAddress;
  if (argc > 1)
    {
      username = argv[1];
      if (argc > 2)
        {
          password = argv[2];
        }
    }
  if (username.empty())
    {
      std::cout << "YOUR JID: ";
      std::cin >> username;
    }

  if (password.empty() && !test_server)
    {
      //Console::SetEcho(false);
      std::cout << "Password: ";
      std::cin >>  password;
      //Console::SetEcho(true);
      std::cout << std::endl;
    }

  //printf("Logging in to %s as %s\n", server.c_str(), jid.Str().c_str());

  CQEMotorTraj &motor = CQEMotorTraj::GetRef();

  // set PID gains for axis
  motor.SetPIDVGains(left_wheel, 100, 0, 500, 0);
  motor.SetPIDVGains(right_wheel, 100, 0, 500, 0);

  XMPPCallback *xmppCallback = new TesterMain( username, password, server, debug, msgsize, msginterval);

  xmppCallback = NULL;

  return 0;
}


